#include <ros/ros.h>
#include <ros/time.h>
#include <unistd.h>
#include <data_record/Cmd.h>
#include <data_record/State.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

using namespace std;

typedef message_filters::sync_policies::ApproximateTime<data_record::Cmd, data_record::State> MySyncPolicy;

class Record
{

public:
    Record();
    ~Record();
    bool init();
    
    void RecordCallback(const data_record::Cmd::ConstPtr &cmd_msg,
                  const data_record::State::ConstPtr &state_msg);

private:
    message_filters::Subscriber<data_record::Cmd> *sub_cmd_;
    message_filters::Subscriber<data_record::State> *sub_state_;
    message_filters::Synchronizer<MySyncPolicy> *m_sync_;
    
	string cmd_speed_;
	string state_speed_;
	string file_name_;
	string file_path_;
	
	float sample_time_;
	double last_sample_time_;
	double current_sample_time_;
	
	FILE *fp;
};

Record::Record()
{
    last_sample_time_ = 0;
    current_sample_time_ = last_sample_time_;
}

bool Record::init()
{
//    ros::NodeHandle nh_();
    ros::NodeHandle private_nh("~");
    
    cmd_speed_ = private_nh.param<std::string>("record_cmd", "/cmd_speed");
    state_speed_ = private_nh.param<std::string>("record_state", "/state_speed");
    sample_time_ = private_nh.param<float>("sample_time", 0.1);
    file_path_ = private_nh.param<std::string>("file_path", "");
    file_name_ = private_nh.param<std::string>("file_name", "");
    
	sub_cmd_ = new message_filters::Subscriber<data_record::Cmd>(private_nh, cmd_speed_, 1, ros::TransportHints().tcpNoDelay());
	sub_state_ = new message_filters::Subscriber<data_record::State>(private_nh, state_speed_, 1, ros::TransportHints().tcpNoDelay());
	
	m_sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10), *sub_cmd_, *sub_state_);
	m_sync_->registerCallback(boost::bind(&Record::RecordCallback, this, _1, _2));
    
	fp = fopen((file_path_+file_name_).c_str(),"w");
	
	if(fp == NULL)
	{
	    ROS_ERROR("open file %s failed!", (file_path_+file_name_).c_str());
	    return false;
	}
	return true;
}

Record::~Record()
{

	if(fp != NULL)
	    fclose(fp);
	    
	delete sub_cmd_;
	delete sub_state_;
	delete m_sync_;
}

void Record::RecordCallback(const data_record::Cmd::ConstPtr &cmd_msg, const data_record::State::ConstPtr &state_msg)
{
    static int index = 0;
    float cmd_speed = cmd_msg->speed;
    float state_speed = state_msg->speed;
    current_sample_time_ = ros::Time::now().toSec();
    if(current_sample_time_ - last_sample_time_ >= sample_time_)
    {
        static double time = 0;
        fprintf(fp,"%0.3f\t%.3f\t%.3f\n",time,cmd_speed,state_speed);
        fflush(fp);
        
        ROS_INFO("index:%d\t%.3f\t%.3f",index++,cmd_speed,state_speed);
        last_sample_time_ = current_sample_time_;
        time += sample_time_;
    }
}

int main(int argc,char**argv)
{
	ros::init(argc,argv,"record_node");
	
	Record record;
	record.init();
	while(ros::ok())
	{
		ros::spin();
	}
	return 0;
}
